/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package SIFT.util;

import java.util.Collection;
import java.util.List;
import java.util.Vector;
import SIFT.util.Util;

/**
 *
 * @author gloria
 */
public class FloatArray2DSIFT2Points  extends FloatArray2DFeatureTransform< FloatArray2DSIFT.Param >{

        float sigmas[];
        float octaveSigmas[];
        float scales[];
        float maxScale;
        float orientations[];
        int sigmaIdx[];
        float points[][];
        FloatArray2D gradients[][][];
        final private int fdWidth;
	final private float fdBinWidth;
        final private float descriptorMask[][];
        final private int nPix;
        final private int nPix2;
        FloatArray2D[] L;

        /**
	 * Constructor
	 *
	 * @param feature_descriptor_size
	 * @param feature_descriptor_size
	 */
	public FloatArray2DSIFT2Points( final FloatArray2DSIFT.Param p )
	{
            super(p);

            sigmas = null;
            orientations = null;
            sigmaIdx = null;
            points = null;
            gradients = null;

            nPix = p.nPix;
            nPix2=nPix*nPix;
            
            fdWidth = nPix * p.fdSize;
            fdBinWidth = 2.0f * ( float )Math.PI / ( float )p.fdBins;

            descriptorMask = new float[ fdWidth ][ fdWidth ];
            
            final float two_sq_sigma = (fdWidth*fdWidth)/2.0f; // sigma = (fdWidth/2)
            final float wm2 = ((float)fdWidth-1)/2.0f;
            float sum = 0f;
            for ( int y =fdWidth- 1; y >= 0; --y )
            {
                    float fy = ( float )y - wm2;
                    for ( int x = fdWidth - 1; x >= 0; --x )
                    {
                            final float fx = ( float )x - wm2;
                            final float val = ( float )Math.exp( -( fy * fy + fx * fx ) / two_sq_sigma );
                            descriptorMask[y][x] = val;
                            sum+=val;
                    }
            }

            for (int y =fdWidth- 1; y >= 0; --y )
            {
                    for ( int x = fdWidth - 1; x >= 0; --x )
                    {
                            descriptorMask[y][x]/=sum;
                    }
            }
        }

        public void setPoints(float p_points[][])
        {
            points = p_points;
        }

        public void setSigmaOctaves(float p_sigmas[])
        {
            octaveSigmas = p_sigmas;
        }

        public void setSigmas(float p_sigmas[])
        {
            sigmas = p_sigmas;
        }

        public void setOrientatios(float p_orientations[])
        {
            orientations = p_orientations;
        }

         public void setScales(float p_orientations[])
        {
            scales = p_orientations;
        }

          /**
	 * initialize the scale space as a scale pyramid having octave stubs only
	 *
	 * @param src image having a generating gaussian kernel of
	 * 	{@link Param#initialSigma} img must be a 2d-array of float
	 *  values in range [0.0f, ..., 1.0f]
	 */
	@Override
	final public void init( FloatArray2D src )
	{
            L = new FloatArray2D[1];
            if(sigmas==null)
            {
                sigmas = new float[1];
                octaveSigmas = new float[1];
                
                float s2 = (p.initialSigma*p.initialSigma);
                octaveSigmas[0]=sigmas[0] = 0.5f;
                float gaussainKernel[] = Filter.createGaussianKernel((float)Math.sqrt(s2-0.25),true);

		L[0] = Filter.convolveSeparable( src, gaussainKernel,gaussainKernel);
                gradients = new FloatArray2D[1][1][];
                sigmaIdx = new int[points.length];
                maxScale = 1;
            }
            else
            {                
                L[0] = src;
                sigmaIdx = new int[points.length];
                
                Vector<Float> tmpSigmas = new Vector<Float>();
                tmpSigmas.add(new Float(octaveSigmas[0]));
                sigmaIdx[0] = 0;
               
                int i;
                int idx;
                Float data;
                for(i=1;i<sigmas.length;i++)
                {
                    data = new Float(octaveSigmas[i]);
                    idx = tmpSigmas.indexOf(data);
                    if(idx<0)
                    {
                        idx = tmpSigmas.size();
                        tmpSigmas.add(data);
                    }

                    sigmaIdx[i] = idx;
                    //System.out.println(java.lang.Integer.toString(idx));
                }
                //System.out.println("Salio ... ");
                maxScale = 1;
                for(i=1;i<scales.length;i++)
                {
                    if(maxScale<scales[i])
                        maxScale = scales[i];
                }
                //System.out.println("Salio ... ");

                octaveSigmas = new float[tmpSigmas.size()];
                for(i=0;i<octaveSigmas.length;i++)
                {
                    octaveSigmas[i] = tmpSigmas.elementAt(i);
                }

                int scaleLegth = (int)(Math.log(maxScale)/Math.log(2) +1);
                gradients = new FloatArray2D[scaleLegth][octaveSigmas.length][];

            }
        }

        @Override
	final public void extractFeatures( final Collection< Feature > features )
	{
            Vector< Feature > lfeatures = new Vector< Feature >();
            //System.out.println("extractFeatures ...............................");
            int i;
            int idx;
            int sidx;
            float orientation;
            float scale;
            float sigma;
            //System.out.println("Points length "+points.length);
            for (i=0;i<points.length;i++)
            {
                //System.out.println("Point init "+i);
                idx = sigmaIdx[i];
                if(orientations==null)
                    orientation = java.lang.Float.NaN;
                else
                    orientation = orientations[i];

                if(scales==null)
                {
                    scale = 1.0f;
                    sigma = sigmas[0];
                }
                else
                {
                    scale = scales[i];
                    sigma = sigmas[i];
                }

                //System.out.println(java.lang.Float.toString(scale));
                //System.out.println(java.lang.Float.toString(orientation));

                processPoint(points[i],sigma,scale,octaveSigmas[idx],orientation,idx,lfeatures);
                //System.out.println("Point "+i);
            }
            //System.out.println("Features length "+lfeatures.size());
            features.addAll(lfeatures);
	}

        protected void calculeGradient(int scale,int sigma)
        {            
            if(gradients[scale][sigma] == null)
            {                
                float gaussainKernel[];
                int mxScale = (int)(Math.log(maxScale)/Math.log(2)+1);
                //System.out.println("Max " + mxScale + " ");
                if(L.length != mxScale)
                {                    
                    FloatArray2D L0 = L[0];
                    L = new FloatArray2D[mxScale];
                    float s2 = (p.initialSigma*p.initialSigma);
                    gaussainKernel = Filter.createGaussianKernel((float)Math.sqrt(s2-0.25),true);
                    L[0] =  Filter.convolveSeparable(L0,gaussainKernel,gaussainKernel);

                    int i;
                    for(i=1;i<mxScale;i++)
                    {
                        L[i] = new FloatArray2D(L[i-1].width / 2 + L[i-1].width % 2,
                                                L[i-1].height / 2 + L[i-1].height % 2 );
                        
                        gaussainKernel = Filter.createGaussianKernel((float)Math.sqrt(3.0*s2),true);
                        L0 = Filter.convolveSeparable(L[i-1],gaussainKernel,gaussainKernel);
                        FloatArray2DScaleOctave.downsample(L0, L[i]);
                    }
                }
                
                FloatArray2D src;
                if(octaveSigmas[sigma]!=0)
                {
                    src = new FloatArray2D(L[scale].width,L[scale].height);
                    gaussainKernel = Filter.createGaussianKernel(octaveSigmas[sigma],true);
                    src = Filter.convolveSeparable(L[scale],gaussainKernel,gaussainKernel);
                }
                else
                {
                    src = L[scale];
                }
                gradients[scale][sigma] = Filter.createGradients(src);
            }
        }

        final protected void processPoint(
			final float[] point, final float sigma,float scale,float octavesigma,float orientation,int idx,
			final List< Feature > features )
	{           
            // get the gradients in a region arround the keypoints location
            //System.out.println(java.lang.Integer.toString(idx));

            int si =  (int)(java.lang.Math.log(scale)/Math.log(2));

            //System.out.println( "scale " + si);
            //System.out.print(si); System.out.print(" ");System.out.println(idx);
            calculeGradient(si,idx);
            FloatArray2D[] src = gradients[si][idx];
            

            //System.out.println(java.lang.Float.toString(scale));
            //System.out.println(java.lang.Float.toString(orientation));

            if(java.lang.Float.isNaN(orientation))
            {                
                final int ORIENTATION_BINS = 36;
		final float ORIENTATION_BIN_SIZE = 2.0f * ( float )Math.PI / ( float )ORIENTATION_BINS;
		float[] histogram_bins = new float[ ORIENTATION_BINS ];
                FloatArray2D[] gradientROI = new FloatArray2D[ 2 ];

                float pscale[] = new float[2];
                pscale[0] = point[ 0 ]/scale;
                pscale[1] = point[ 1 ]/scale;
                // create a circular gaussian window with sigma 1.5 times that of the feature
		FloatArray2D gaussianMask =
			Filter.createGaussianKernelOffset(
					sigma * 1.5f,
					pscale[ 0 ] - ( float )Math.floor( pscale[ 0 ] ),
					pscale[ 1 ] - ( float )Math.floor( pscale[ 1 ] ),
                                        false);
		
		gradientROI[ 0 ] = new FloatArray2D( gaussianMask.width, gaussianMask.width );
		gradientROI[ 1 ] = new FloatArray2D( gaussianMask.width, gaussianMask.width );

		int half_size = gaussianMask.width / 2;
		int p = gaussianMask.width * gaussianMask.width - 1;
		for ( int yi = gaussianMask.width - 1; yi >= 0; --yi )
		{
			int ra_y = src[ 0 ].width * Math.max( 0, Math.min( src[ 0 ].height - 1, ( int )pscale[ 1 ] + yi - half_size ) );
			int ra_x = ra_y + Math.min( ( int )pscale[ 0 ], src[ 0 ].width - 1 );

			for ( int xi = gaussianMask.width - 1; xi >= 0; --xi )
			{
				int pt = Math.max( ra_y, Math.min( ra_y + src[ 0 ].width - 2, ra_x + xi - half_size ) );
				gradientROI[ 0 ].data[ p ] = src[ 0 ].data[ pt ];
				gradientROI[ 1 ].data[ p ] = src[ 1 ].data[ pt ];
				--p;
			}
		}

		// and mask this region with the precalculated gaussion window
		for ( int i = 0; i < gradientROI[ 0 ].data.length; ++i )
		{
			gradientROI[ 0 ].data[ i ] *= gaussianMask.data[ i ];
		}

		// TODO this is for test
		//---------------------------------------------------------------------
		//ImageArrayConverter.FloatArrayToImagePlus( gradientROI[ 0 ], "gaussianMaskedGradientROI", 0, 0 ).show();
		//ImageArrayConverter.FloatArrayToImagePlus( gradientROI[ 1 ], "gaussianMaskedGradientROI", 0, 0 ).show();

		// build an orientation histogram of the region
		for ( int i = 0; i < gradientROI[ 0 ].data.length; ++i )
		{
			int bin = Math.max( 0, ( int )( ( gradientROI[ 1 ].data[ i ] + Math.PI ) / ORIENTATION_BIN_SIZE ) );
			histogram_bins[ bin ] += gradientROI[ 0 ].data[ i ];
		}

		// find the dominant orientation and interpolate it with respect to its two neighbours
		int max_i = 0;
		for ( int i = 0; i < ORIENTATION_BINS; ++i )
		{
			if ( histogram_bins[ i ] > histogram_bins[ max_i ] )
                            max_i = i;
		}

                //System.out.print("O org "+orientation+" - ");
		/**
		 * interpolate orientation estimate the offset from center of the
		 * parabolic extremum of the taylor series through env[1], derivatives
		 * via central difference and laplace
		 */
		float e0 = histogram_bins[ ( max_i + ORIENTATION_BINS - 1 ) % ORIENTATION_BINS ];
		float e1 = histogram_bins[ max_i ];
		float e2 = histogram_bins[ ( max_i + 1 ) % ORIENTATION_BINS ];
		float offset = ( e0 - e2 ) / 2.0f / ( e0 - 2.0f * e1 + e2 );
                if(java.lang.Float.isNaN(offset))
                    orientation = 0;
                else
                    orientation = ( ( float )max_i + offset ) * ORIENTATION_BIN_SIZE - ( float )Math.PI;

                //System.out.println("O calc "+orientation+" *");
        }        

        // assign descriptor and add the Feature instance to the collection
        features.add(new Feature(
                                sigma* scale,
                                scale,
                                sigma,
                                octavesigma,
                                orientation,
                                new float[]{ point[ 0 ], point[ 1 ]},
                                //new float[]{ ( c[ 0 ] + 0.5f ) * scale - 0.5f, ( c[ 1 ] + 0.5f ) * scale - 0.5f },
                                createDescriptor( point, src,sigma,scale,orientation ) ) );
		
	}

        /**
	 * sample the scaled and rotated gradients in a region around the
	 * features location, the regions size is defined by
	 * ( FEATURE_DESCRIPTOR_SIZE * 4 )^2 ( 4x4 subregions )
	 *
	 * @param c candidate 0=>x, 1=>y, 2=>scale index
	 * @param o octave index
	 * @param octave_sigma sigma of the corresponding gaussian kernel with
	 *   respect to the scale octave
	 * @param orientation orientation [-&pi; ... &pi;]
	 */
	private float[] createDescriptor(
			float[] point,  FloatArray2D[] gradients,
			float sigma,float scale,
			float orientation )
	{				
		FloatArray2D[] region = new FloatArray2D[ 2 ];

		region[ 0 ] = new FloatArray2D(
				fdWidth,
				fdWidth );
		region[ 1 ] = new FloatArray2D(
				fdWidth,
				fdWidth );
		float cos_o = ( float )Math.cos( orientation );
		float sin_o = ( float )Math.sin( orientation );

		// TODO this is for test
		//---------------------------------------------------------------------
		//FloatArray2D image = octave.getL( Math.round( c[ 2 ] ) );
		//pattern = new FloatArray2D( FEATURE_DESCRIPTOR_WIDTH, FEATURE_DESCRIPTOR_WIDTH );

		//! sample the region arround the keypoint location
		for ( int y = fdWidth - 1; y >= 0; --y )
		{
			float ys =
				( ( float )y - 2.0f * ( float )p.fdSize + 0.5f ) * sigma; //!< scale y around 0,0
			for ( int x = fdWidth - 1; x >= 0; --x )
			{
				float xs =
					( ( float )x - 2.0f * ( float )p.fdSize + 0.5f ) * sigma; //!< scale x around 0,0
				float yr = cos_o * ys + sin_o * xs; //!< rotate y around 0,0
				float xr = cos_o * xs - sin_o * ys; //!< rotate x around 0,0

				// flip_range at borders
				// TODO for now, the gradients orientations do not flip outside
				// the image even though they should do it. But would this
				// improve the result?

				// translate ys to sample y position in the gradient image
				int yg = Util.pingPong(
						( int )( Math.round( yr + (point[ 1 ]/scale) ) ),
						gradients[ 0 ].height );

				// translate xs to sample x position in the gradient image
				int xg = Util.pingPong(
						( int )( Math.round( xr + (point[ 0 ]/scale) ) ),
						gradients[ 0 ].width );

				// get the samples
				int region_p = fdWidth * y + x;
				int gradient_p = gradients[ 0 ].width * yg + xg;

				// weigh the gradients
				region[ 0 ].data[ region_p ] = gradients[ 0 ].data[ gradient_p ] * descriptorMask[ y ][ x ];

				// rotate the gradients orientation it with respect to the features orientation
				region[ 1 ].data[ region_p ] = gradients[ 1 ].data[ gradient_p ] - orientation;
                                
				// TODO this is for test
				//---------------------------------------------------------------------
				//pattern.data[ region_p ] = image.data[ gradient_p ];
			}
		}



		final float[][][] hist = new float[ p.fdSize ][ p.fdSize ][ p.fdBins ];

		float theta;
                // build the orientation histograms of 4x4 subregions (nPix X nPix)
		for ( int y = p.fdSize - 1; y >= 0; --y )
		{
			int yp = p.fdSize * nPix2 * y;
			for ( int x = p.fdSize - 1; x >= 0; --x )
			{
				int xp = nPix * x;
				for ( int ysr = (nPix-1); ysr >= 0; --ysr )
				{
					int ysrp = nPix * p.fdSize * ysr;
					for ( int xsr = (nPix-1); xsr >= 0; --xsr )
					{
						theta = ( region[ 1 ].data[ yp + xp + ysrp + xsr ] + ( float )Math.PI );
                                                if(theta<0)
                                                    theta = ( float )(Math.PI*2) + theta;
                                                else if(theta > (Math.PI*2))
                                                    theta = theta + ( float )(Math.PI*2);

                                                float bin_location =  theta/ ( float )fdBinWidth;

						int bin_b = ( int )( bin_location );
						int bin_t = bin_b + 1;
						float d = bin_location - ( float )bin_b;
                                               
						//bin_b = ( bin_b + 2 * p.fdBins ) % p.fdBins;
						//bin_t = ( bin_t + 2 * p.fdBins ) % p.fdBins;
                                                bin_b =  bin_b  % p.fdBins;
						bin_t =  bin_t  % p.fdBins;

						float t = region[ 0 ].data[ yp + xp + ysrp + xsr ];

						hist[ y ][ x ][ bin_b ] += t * ( 1 - d );
						hist[ y ][ x ][ bin_t ] += t * d;
					}
				}
			}
		}

		final float[] desc = new float[ p.fdSize * p.fdSize * p.fdBins ];

		// normalize, cut above 0.2 and renormalize
		float max_bin_val = 0;
		int i = 0;
		for ( int y = p.fdSize - 1; y >= 0; --y )
		{
			for ( int x = p.fdSize - 1; x >= 0; --x )
			{
				for ( int b = p.fdBins - 1; b >= 0; --b )
				{
					desc[ i ] = hist[ y ][ x ][ b ];
					if ( desc[ i ] > max_bin_val )
                                            max_bin_val = desc[ i ];
					++i;
				}
			}
		}
		max_bin_val /= 0.2;
		for ( i = 0; i < desc.length; ++i )
		{
                    if(max_bin_val==0)
                        desc[ i ] = 0;
                    else
                        desc[ i ] = ( float )Math.min( 1.0, desc[ i ] / max_bin_val );
		}

		return desc;
	}		      
}
